import cv2




cap = cv2.VideoCapture(0)

# 检查摄像头是否打开
if not cap.isOpened():
    print("无法打开摄像头")
    exit()

while True:
    # 从摄像头读取视频帧
    ret, frame = cap.read()
    
    # 检查视频帧是否读取成功
    if not ret:
        print("无法读取视频帧")
        break
    
    # 转换为灰度图
    gray = cv2.cvtColor(frame, cv2.COLOR_BGR2GRAY)
    # Gausi blur
    blur = cv2.GaussianBlur(gray,(5,5),0)
    #to binary
    """
    cv2.THRESH_BINARY：大于阈值的像素赋值为 maxval，小于等于阈值的像素赋值为 0。
    cv2.THRESH_BINARY_INV：大于阈值的像素赋值为 0，小于等于阈值的像素赋值为 maxval。
    """
    ret,binary = cv2.threshold(blur,127,255,cv2.THRESH_BINARY_INV)
    # #Close
    # kernel = cv2.getStructuringElement(cv2.MORPH_RECT, (17,17))
    # close = cv2.morphologyEx(binary, cv2.MORPH_CLOSE, kernel)
    # 寻找图像中的轮廓
    contours, _ = cv2.findContours(binary, cv2.RETR_EXTERNAL, cv2.CHAIN_APPROX_SIMPLE)
    print(contours)
    print("----------------")

# 在原始图像上绘制轮廓
    cv2.drawContours(frame, contours, -1, (0, 255, 0), 2)   
    


    # 显示视频帧
    cv2.imshow('frame', frame)
    


    # 检查是否按下 'q' 键，如果是则退出循环
    key = cv2.waitKey(1) & 0xFF
    if  key== ord('q'):
        break

    elif key == ord('s'):
        print("hello")
        pass
    elif key == ord('a'):
        print("press a")

# 释放摄像头并关闭窗口
cap.release()
cv2.destroyAllWindows()



"""
轨迹识别+跟踪流程：
首先读取图像
对图像进行处理、包括灰度、二值化处理、识别出轨迹:只剩下轨迹（白）、其他（黑）
绘制出轨迹轮廓，同时找到轨迹在像素中的位置
无人机根据像素中的轨迹的位置进行调整（包括姿态和位姿）、使轨迹像素点始终保持在中心
无人机稳定飞行

流程：
解锁、起飞、悬停
轨迹跟踪飞行 5圈
结束、降落、上锁

"""

